package com.yuneec.droneservice.command;

import com.MAVLink.yuneec.msg_mav_rc_cmd_ack;
import com.yuneec.droneservice.request.IResponse;

/**
 * Created by gjw on 2017/3/27.
 */

public class GetChannelFormula extends BaseCommand {
    private final int index;

    public GetChannelFormula(int index) {
        super(MAV_RC_COMMAND.MAV_RC_CMD_GET_CH_CURVE);
        this.index = index;
    }

    @Override
    protected short[] body() {
        return new short[]{(short) index};
    }

    @Override
    public IResponse toResponse(msg_mav_rc_cmd_ack ack) {
        int code = ack.code;
        IResponse<int[]> response = new IResponse(ids(), code);
        if (code == MAV_RC_ERROR_CODE.MAV_RC_ERR_SUCCESS) {
            try {
                int[] formula = new int[17];
                int len = ack.params == null ? 0 : ack.params.length;
                for (int i = 0; i < len; ) {
                    int index = i / 2;
                    if (index >= 17) {
                        break;
                    }
                    formula[index] = uIntArrayToUShort(ack.params, i);
                    i += 2;
                }
                response.setParam(formula);
            } catch (Exception e) {
                response.setCode(MAV_RC_ERROR_CODE.MAV_RC_ERR_UNKNOW);
            }
        }
        return response;
    }

}
